#ifndef FOLLOWPATH_H
#define FOLLOWPATH_H

//All includes
#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/Encoder.h"
#include "roboard_drivers/Motor.h"
#include "saint_nr_9/MotCom.h"
#include "saint_nr_9/IRdata.h"
#include "saint_nr_9/MotCorrect.h"
#include <unistd.h>
#include <math.h>
#include <stdio.h>
#include "StateMachine.h"
#include "MotorControl.h"

#define DIFFERENCE_IS_WALL 3
#define DISTANCE_IS_WALL  18

#define SPEED_SLOW        0.4
#define SPEED_WALL        0.6
#define SPEED_TURN        0.4

void state_update(const std_msgs::Int32::ConstPtr& state);
void ir_callback(const saint_nr_9::IRdata::ConstPtr& data);

void state_straight();
void state_left();
void state_right();
void state_u_turn();
void search_tag();


#endif
